#include <stdio.h>
#include <stdbool.h>
#include <time.h>

#include "hi_common.h"
#include "hi_comm_sys.h"
#include "hi_comm_svp.h"
#include "sample_comm.h"
#include "sample_comm_svp.h"
#include "sample_nnie_main.h"
#include "sample_comm_ive.h"

#include "hisignalling.h"
//#include "vpss_capture.h"
#include "yolo_sample.h"
#include "sample_store_rtmp.h"

#include "sample_comm_nnie.h"
//#include "sample_svp_nnie_software.h"
//#include "hi_comm_vpss.h"

#include "config.h"

/********************************** YOLOV5 parm ******************************************/
static SAMPLE_SVP_NNIE_MODEL_S s_stYOLOv5Model = { 0 };
//static SAMPLE_SVP_NNIE_MODEL_S s_stExtModel = {0};
static SAMPLE_SVP_NNIE_PARAM_S s_stYOLOv5NnieParam = { 0 };
//static SAMPLE_SVP_NNIE_PARAM_S s_stExtNnieParam = {0};

/********************************** end of parm definition ******************************************/

IVE_DST_IMAGE_S stDst;
VPSS_GRP VpssGrp = 0;
VPSS_CHN VpssChn = 1;
VENC_CHN VencChn[2] = {0, 1};


HI_U32 u32OrigDepth = 0;

HI_S32 s32MilliSec = -1;
HI_U32 u32VpssDepthFlag = 0;
HI_U32 u32SignalFlag = 0;
VIDEO_FRAME_INFO_S stFrame;
VIDEO_FRAME_INFO_S stFrame_vo;
HI_U32  u32BlkSize = 0;
VGS_HANDLE hHandle = -1;
//DUMP_MEMBUF_S stMem = {0};
VB_POOL hPool  = VB_INVALID_POOLID;
HI_U32 u32Size = 0;
HI_CHAR* pUserPageAddr[2] = {HI_NULL, HI_NULL};
unsigned int frame_id = 0;
clock_t start, finish;

HI_S32 S_frame_flag = 0; //用于抽帧推理
boxes_buf *S_frame_Rect = NULL;

int uartFd = 0;
int RectNum[2] = {0,0};
int RectNumTemp;
int State = 0;
int StabFlag = 0;
int WaitFlag = 0;
int SendFlag = 0;


/* function : NNIE Forward */
static HI_S32 SAMPLE_SVP_NNIE_Forward(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam,
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S *pstInputDataIdx, SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S *pstProcSegIdx,
    HI_BOOL bInstant)
{
    HI_S32 s32Ret = HI_SUCCESS;
    HI_U32 i, j;
    HI_BOOL bFinish = HI_FALSE;
    SVP_NNIE_HANDLE hSvpNnieHandle = 0;
    HI_U32 u32TotalStepNum = 0;

    SAMPLE_SVP_CHECK_EXPR_RET(pstProcSegIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstInputDataIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstNnieParam->pstModel->u32NetSegNum > SVP_NNIE_MAX_NET_SEG_NUM,
        HI_INVALID_VALUE, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error, pstProcSegIdx->u32SegIdx(%u) and pstInputDataIdx->u32SegIdx(%u) "
        "should be less than %u, pstNnieParam->pstModel->u32NetSegNum(%u) can't be greater than %u!\n",
        pstProcSegIdx->u32SegIdx, pstInputDataIdx->u32SegIdx, pstNnieParam->pstModel->u32NetSegNum,
        pstNnieParam->pstModel->u32NetSegNum, SVP_NNIE_MAX_NET_SEG_NUM);

    SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64PhyAddr,
        SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
        pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64VirAddr),
        pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u32Size);

    for (i = 0; i < pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType == SVP_BLOB_TYPE_SEQ_S32) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    /* set input blob according to node name */
    if (pstInputDataIdx->u32SegIdx != pstProcSegIdx->u32SegIdx) {
        for (i = 0; i < pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].u16SrcNum; i++) {
            for (j = 0; j < pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum; j++) {
                if (strncmp(pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].astDstNode[j].szName,
                    pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].astSrcNode[i].szName,
                    SVP_NNIE_NODE_NAME_LEN) == 0) {
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc[i] =
                        pstNnieParam->astSegData[pstInputDataIdx->u32SegIdx].astDst[j];
                    break;
                }
            }
            SAMPLE_SVP_CHECK_EXPR_RET((j == pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum),
                HI_FAILURE, SAMPLE_SVP_ERR_LEVEL_ERROR, "Error,can't find %d-th seg's %d-th src blob!\n",
                pstProcSegIdx->u32SegIdx, i);
        }
    }

    /* NNIE_Forward */
    s32Ret = HI_MPI_SVP_NNIE_Forward(&hSvpNnieHandle, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc,
        pstNnieParam->pstModel, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst,
        &pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx], bInstant);
    SAMPLE_SVP_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_Forward failed!\n");

    if (bInstant) {
        /* Wait NNIE finish */
        while (HI_ERR_SVP_NNIE_QUERY_TIMEOUT == (s32Ret = HI_MPI_SVP_NNIE_Query(
            pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].enNnieId, hSvpNnieHandle, &bFinish, HI_TRUE))) {
            usleep(100); /* sleep 100 micro_seconds */
            SAMPLE_SVP_TRACE(SAMPLE_SVP_ERR_LEVEL_INFO, "HI_MPI_SVP_NNIE_Query Query timeout!\n");
        }
    }
    u32TotalStepNum = 0;
    for (i = 0; i < pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    return s32Ret;
}

/* function : NNIE ForwardWithBbox */
static HI_S32 SAMPLE_SVP_NNIE_ForwardWithBbox(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam,
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S *pstInputDataIdx, SVP_SRC_BLOB_S astBbox[],
    SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S *pstProcSegIdx, HI_BOOL bInstant)
{
    HI_S32 s32Ret = HI_SUCCESS;
    HI_BOOL bFinish = HI_FALSE;
    SVP_NNIE_HANDLE hSvpNnieHandle = 0;
    HI_U32 u32TotalStepNum = 0;
    HI_U32 i, j;

    SAMPLE_SVP_CHECK_EXPR_RET(pstProcSegIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstInputDataIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstNnieParam->pstModel->u32NetSegNum > SVP_NNIE_MAX_NET_SEG_NUM,
        HI_INVALID_VALUE, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error, pstProcSegIdx->u32SegIdx(%u) and pstInputDataIdx->u32SegIdx(%u) "
        "should be less than %u, pstNnieParam->pstModel->u32NetSegNum(%u) should be less than %u!\n",
        pstProcSegIdx->u32SegIdx, pstInputDataIdx->u32SegIdx, pstNnieParam->pstModel->u32NetSegNum,
        pstNnieParam->pstModel->u32NetSegNum, SVP_NNIE_MAX_NET_SEG_NUM);
    SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64PhyAddr,
        SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
        pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64VirAddr),
        pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u32Size);

    for (i = 0; i < pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    /* set input blob according to node name */
    if (pstInputDataIdx->u32SegIdx != pstProcSegIdx->u32SegIdx) {
        for (i = 0; i < pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].u16SrcNum; i++) {
            for (j = 0; j < pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum; j++) {
                if (strncmp(pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].astDstNode[j].szName,
                    pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].astSrcNode[i].szName,
                    SVP_NNIE_NODE_NAME_LEN) == 0) {
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc[i] =
                        pstNnieParam->astSegData[pstInputDataIdx->u32SegIdx].astDst[j];
                    break;
                }
            }
            SAMPLE_SVP_CHECK_EXPR_RET((j == pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum),
                HI_FAILURE, SAMPLE_SVP_ERR_LEVEL_ERROR, "Error,can't find %d-th seg's %d-th src blob!\n",
                pstProcSegIdx->u32SegIdx, i);
        }
    }
    /* NNIE_ForwardWithBbox */
    s32Ret = HI_MPI_SVP_NNIE_ForwardWithBbox(&hSvpNnieHandle, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc,
        astBbox, pstNnieParam->pstModel, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst,
        &pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx], bInstant);
    SAMPLE_SVP_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_ForwardWithBbox failed!\n");

    if (bInstant) {
        /* Wait NNIE finish */
        while (HI_ERR_SVP_NNIE_QUERY_TIMEOUT ==
            (s32Ret = HI_MPI_SVP_NNIE_Query(pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].enNnieId,
            hSvpNnieHandle, &bFinish, HI_TRUE))) {
            usleep(100); /* sleep 100 micro_seconds */
            SAMPLE_SVP_TRACE(SAMPLE_SVP_ERR_LEVEL_INFO, "HI_MPI_SVP_NNIE_Query Query timeout!\n");
        }
    }
    u32TotalStepNum = 0;

    for (i = 0; i < pstNnieParam->astForwardWithBboxCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    return s32Ret;
}



static HI_S32 SAMPLE_SVP_NNIE_YOLOv5_Deinit(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam, SAMPLE_SVP_NNIE_MODEL_S* pstNnieModel)
{
		
	HI_S32 s32Ret = HI_SUCCESS;
	/*hardware para deinit*/
	if(pstNnieParam!=NULL)
	{
		s32Ret = SAMPLE_COMM_SVP_NNIE_ParamDeinit(pstNnieParam);
		SAMPLE_SVP_CHECK_EXPR_TRACE(HI_SUCCESS != s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
			"Error,SAMPLE_COMM_SVP_NNIE_ParamDeinit failed!\n");
	}
	/*model deinit*/
	if(pstNnieModel!=NULL)
	{
		s32Ret = SAMPLE_COMM_SVP_NNIE_UnloadModel(pstNnieModel);
		SAMPLE_SVP_CHECK_EXPR_TRACE(HI_SUCCESS != s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
			"Error,SAMPLE_COMM_SVP_NNIE_UnloadModel failed!\n");
	}
	return s32Ret;
}



static HI_S32 SAMPLE_SVP_NNIE_YOLOv5_ParamInit(SAMPLE_SVP_NNIE_CFG_S* pstNnieCfg,
	SAMPLE_SVP_NNIE_PARAM_S *pstYOLOv5Para)
{
	HI_S32 s32Ret = HI_SUCCESS;
	/*init hardware para*/
	s32Ret = SAMPLE_COMM_SVP_NNIE_ParamInit(pstNnieCfg,pstYOLOv5Para); // 初始化 NNIE 硬件层参数 。
	SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret,INIT_FAIL_0,SAMPLE_SVP_ERR_LEVEL_ERROR,
		"Error(%#x),SAMPLE_COMM_SVP_NNIE_ParamInit failed!\n",s32Ret);
	
	return s32Ret;
INIT_FAIL_0:
	s32Ret = SAMPLE_SVP_NNIE_YOLOv5_Deinit(pstYOLOv5Para, NULL); // 如果失败，反初始化
	SAMPLE_SVP_CHECK_EXPR_RET(HI_SUCCESS != s32Ret,s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
			"Error(%#x),SAMPLE_SVP_NNIE_YOLOv5_Deinit failed!\n",s32Ret);
	return HI_FAILURE;
	
}



	float sigmoid(float x)
	{
		return (1.0f / ((float)exp((double)(-x)) + 1.0f));
	}



	static unsigned int yolov5_result_process(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam, float *strides, anchor_w_h (*anchor_grids)[3], int *map_size, yolo_result **output_result, float confidence_threshold)
	{
		HI_S32 output_num = 0;
		
		HI_S32 anchor_num = 0;
		
		HI_S32 feature_length = 0;
	
		float anchor_w = 0.0f;
	
		float anchor_h = 0.0f;
	
		int x = 0;
		
		int y = 0;
	
		HI_S32* output_addr = NULL;
	
		float confidence = 0.0f;
	
		float class_confidence = 0.0f;
	
		//float confidence_threshold = 0.4f;
	
		yolo_result *current = NULL;
		
		yolo_result *former = NULL;
		
		*output_result = NULL;
	
		unsigned int resltu_num = 0;
		for (int yolo_layer_index = 0; yolo_layer_index < yolo_layer_num; yolo_layer_index ++) { // 3 yolo layer 
		
			feature_length = pstNnieParam->astSegData[0].astDst[yolo_layer_index].unShape.stWhc.u32Width; // 1600 / 400 / 100
			
			output_num = pstNnieParam->astSegData[0].astDst[yolo_layer_index].unShape.stWhc.u32Height; // 53
			
			anchor_num = pstNnieParam->astSegData[0].astDst[yolo_layer_index].unShape.stWhc.u32Chn; // 3
	
			//printf("HSJ Debug:In result process feature_length:%d ;output_num53:%d; anchor_num3:%d \n",feature_length,output_num,anchor_num); //HSJ Debug
			
			output_addr = (HI_S32* )((HI_U8* )pstNnieParam->astSegData[0].astDst[yolo_layer_index].u64VirAddr); // yolo 输出的首地址
			
			//printf("HSJ Debug:output_addr:%p \n",output_addr); //HSJ Debug
			
			for (int anchor_index = 0; anchor_index < anchor_num; anchor_index ++){ // 每个 grid 上有三个 anchor
					
					anchor_w = anchor_grids[yolo_layer_index][anchor_index].anchor_w;
					anchor_h = anchor_grids[yolo_layer_index][anchor_index].anchor_h;
					}
								
		return resltu_num;
	}
	
	
	void yolov5_result_sort(yolo_result *output_result){ // 目前用这个做排序
		
		yolo_result *comparable_node = NULL; // 右节点，挨个指向右边所有节点
		
		yolo_result *comparable_next_node = NULL;
		
		yolo_result *current_node = output_result; // 左节点，其与右边每个节点做比较
	
		yolo_result *current_next_node = NULL;
		
		yolo_result temp_node = {0};
		
		while (current_node != NULL){
		
			comparable_node = current_node->next;
		
			current_next_node = current_node->next; // 记录后续节点，方便调换数据后维持链表完整
		
			while (comparable_node != NULL){
				
				comparable_next_node = comparable_node->next; // 记录后续节点，方便调换数据后维持链表完整
				
				if (current_node->score >= comparable_node->score){ // 如果大于它，说明后面的比它小，比较下一个
					
					comparable_node = comparable_node->next;
					
				}else{
					// 当大于 current_confidence 时，数据做调换，内存不变，小的放后面去
					memcpy(&temp_node, current_node, sizeof(yolo_result));
					
					memcpy(current_node, comparable_node, sizeof(yolo_result));
					
					memcpy(comparable_node, &temp_node, sizeof(yolo_result));
					
					current_node->next = current_next_node; // 链表接好
					
					comparable_node->next = comparable_next_node;
	
					comparable_node = comparable_node->next; //更新位置，因为当前节点已经小于current_node ，不必再做比较
				}
				
			}
			
			current_node = current_node->next;
		}
	}
	
	
	
	
	
	
	HI_U32 coordinate_clip_int(int down, int up, int input) // 不能过界，否则画框时会报错
	{
		input = (input >= down)? input : down;
		input = (input <= up)? input : up;
		return input;
	}
	
	
	

	void printf_result(yolo_result *temp){
		printf("--------------------\n");
	
		while (temp != NULL){
			
			printf("output_result->left_up_x = %f\t", temp->left_up_x);
			printf("output_result->left_up_y = %f\n", temp->left_up_y);
	
			printf("output_result->right_down_x = %f\t", temp->right_down_x);
			printf("output_result->right_down_y = %f\n", temp->right_down_y);
	
			printf("output_result->class_index = %d\t", temp->class_index);
			printf("output_result->score = %f\n\n", temp->score);
			
			temp = temp->next;
		}
		printf("--------------------\n");
	}


	void release_result(yolo_result *output_result, boxes_buf *final_result){
	
		yolo_result *temp1 = NULL;
		boxes_buf *temp2 = NULL;
	
		while (output_result != NULL){
			
			temp1 = output_result;
			
			output_result = output_result->next;
		
			free(temp1);
		}
		while (final_result != NULL){
			
			temp2 = final_result;
			
			final_result = final_result->next;
		
			free(temp2);
		}
	}


	HI_S32 YOLOv5_SAMPLE_COMM_SVP_NNIE_FillRect(VIDEO_FRAME_INFO_S *pstFrmInfo, boxes_buf * pstRect, HI_U32 u32Color)
	{
		VGS_HANDLE VgsHandle = -1;
		HI_S32 s32Ret = HI_SUCCESS;
		HI_S32 i,j;
		VGS_TASK_ATTR_S stVgsTask;
		VGS_ADD_COVER_S stVgsAddCover;
		//static HI_U32 u32Frm = 0;
		//u32Frm++;
		/*
		if (0 == pstRect->u32TotalNum)
		{
			return s32Ret;
		}
		*/
		//printf("HSJ Debug:befor BeginJob!\n");//HSJ Debug
		s32Ret = HI_MPI_VGS_BeginJob(&VgsHandle);
		//printf("HSJ Debug:after BeginJob!\n");//HSJ Debug
		if (s32Ret != HI_SUCCESS)
		{
			SAMPLE_PRT("Vgs begin job fail,Error(%#x)\n", s32Ret);
			return s32Ret;
		}
	
		memcpy(&stVgsTask.stImgIn, pstFrmInfo, sizeof(VIDEO_FRAME_INFO_S));
		memcpy(&stVgsTask.stImgOut, pstFrmInfo, sizeof(VIDEO_FRAME_INFO_S));
	
		stVgsAddCover.enCoverType = COVER_QUAD_RANGLE;
		stVgsAddCover.u32Color = u32Color;
		stVgsAddCover.stQuadRangle.bSolid = HI_FALSE;
		stVgsAddCover.stQuadRangle.u32Thick = 4;

	
		for (i = 0; i < (pstRect->class_index)+1; i++)//(pstRect->class_index)+1:class number
		{
			//printf("sizeof(stVgsAddCover.stQuadRangle.stPoint):%d; sizeof(pstRect->finalrec.astPoint):%d",sizeof(stVgsAddCover.stQuadRangle.stPoint),sizeof(pstRect->finalrec.astPoint));
			memcpy(stVgsAddCover.stQuadRangle.stPoint, pstRect->finalrec.astPoint, sizeof(pstRect->finalrec.astPoint));
			//stVgsAddCover.stQuadRangle.stPoint[i] = pstRect->finalrec.astPoint[i];
			//printf("HSJ Debug:befor AddCoverTask!\n");//HSJ Debug
			s32Ret = HI_MPI_VGS_AddCoverTask(VgsHandle, &stVgsTask, &stVgsAddCover);
			if (s32Ret != HI_SUCCESS)
			{
				SAMPLE_PRT("HI_MPI_VGS_AddCoverTask fail,Error(%#x)\n", s32Ret);
				HI_MPI_VGS_CancelJob(VgsHandle);
				return s32Ret;
			}
	
		}
		
		//printf("HSJ Debug:after for!\n");//HSJ Debug
	
		s32Ret = HI_MPI_VGS_EndJob(VgsHandle);
		if (s32Ret != HI_SUCCESS)
		{
			SAMPLE_PRT("HI_MPI_VGS_EndJob fail,Error(%#x)\n", s32Ret);
			HI_MPI_VGS_CancelJob(VgsHandle);
			return s32Ret;
		}
		//printf("HSJ Debug:befor reture!\n");//HSJ Debug
	
		return s32Ret;
	
	
	}



	HI_S32 yolov5_Proc(VPSS_GRP s32VpssGrp, VPSS_CHN s32VpssChn, VIDEO_FRAME_INFO_S *pstFrame)
	{
		HI_S32 s32Ret;
		SAMPLE_SVP_NNIE_PARAM_S *pstParam;
		//SAMPLE_SVP_NNIE_RFCN_SOFTWARE_PARAM_S *pstSwParam;
		SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S stInputDataIdx = {0};
		SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S stProcSegIdx = {0};	
	
		stInputDataIdx.u32SegIdx = 0;
		stInputDataIdx.u32NodeIdx = 0;
		yolo_result *output_result = NULL;

		if(S_frame_flag == 1){
			while(S_frame_Rect != NULL)
			{
				//printf("HSJ Debug:befor FillRect process:\n");
				s32Ret = YOLOv5_SAMPLE_COMM_SVP_NNIE_FillRect(pstFrame, S_frame_Rect, 0x0000FF00);
				SAMPLE_SVP_CHECK_EXPR_RET(HI_SUCCESS != s32Ret,s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
				"Error,YOLOv5_SAMPLE_SVP_NNIE_FillRect failed!\n");

				S_frame_Rect = S_frame_Rect->next;
			}
			release_result(output_result, S_frame_Rect); // Free memory, end one frame
			S_frame_flag = 0;
			return s32Ret;
		} 
		else{ 
		
			pstParam = &s_stYOLOv5NnieParam;

			pstParam->astSegData[stInputDataIdx.u32SegIdx].astSrc[stInputDataIdx.u32NodeIdx].u64VirAddr = pstFrame->stVFrame.u64VirAddr[0];
			pstParam->astSegData[stInputDataIdx.u32SegIdx].astSrc[stInputDataIdx.u32NodeIdx].u64PhyAddr = pstFrame->stVFrame.u64PhyAddr[0];
			pstParam->astSegData[stInputDataIdx.u32SegIdx].astSrc[stInputDataIdx.u32NodeIdx].u32Stride	= pstFrame->stVFrame.u32Stride[0];

	
			/*NNIE process 0-th seg*/
			stProcSegIdx.u32SegIdx = 0;
			//printf("HSJ Debug:Start Forward!\n");//HSJ Debug
			s32Ret = SAMPLE_SVP_NNIE_Forward(pstParam,&stInputDataIdx,&stProcSegIdx,HI_TRUE);
			SAMPLE_SVP_CHECK_EXPR_RET(HI_SUCCESS != s32Ret,s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
				"Error,SAMPLE_SVP_NNIE_Forward failed!\n");

	
			unsigned int resltu_num = yolov5_result_process(pstParam, strides, anchor_grids, map_size, &output_result, 0.4);  //confidence threshold = 0.4

	
			if(output_result != NULL){
				yolov5_result_sort(output_result); // sort result
	
				//printf("HSJ Debug:after sort process:\n");
	
				//printf_result(output_result);
			
				yolov5_nms(output_result, 0.6);    // nms iou_threshold = 0.6
				//output_result = output_result->next;
			}

		
			boxes_buf *final_result = NULL;
	
			if (output_result != NULL)
			{ // 如果有bbox数据，转换坐标
				//transform rect: 1. to src pic,  2.x1x2x3x4->x1
				s32Ret = yolov5_GetRect(pstFrame, &final_result, output_result, 640, 640);  //the source pix  1920, 1080//2592, 1536

			}
			
			RectNum[1] = RectNum[0];
			RectNum[0] = 0;
			
			while(final_result != NULL)
			{
				//printf("HSJ Debug:befor FillRect process:\n");
				s32Ret = YOLOv5_SAMPLE_COMM_SVP_NNIE_FillRect(pstFrame, final_result, 0x0000FF00);
				SAMPLE_SVP_CHECK_EXPR_RET(HI_SUCCESS != s32Ret,s32Ret,SAMPLE_SVP_ERR_LEVEL_ERROR,
				"Error,YOLOv5_SAMPLE_SVP_NNIE_FillRect failed!\n");

				final_result = final_result->next;
				RectNum[0]++;
			}

			if(State == 0) {  // State 0:waiting the rect stable; 1:find the reduced of rect; 2:waiting the rect stable
				if(RectNum[0] == RectNum[1]) {
					StabFlag++;
				} else {
					StabFlag = 0;
				}
				if(StabFlag == 5) {
					RectNumTemp = RectNum[0];
					StabFlag = 0;
					State = 1; //The number of Rect stabilized, and start detect the reduced of rect
					printf("start detect the reduced of rect!!! \n");
				}
			} else
			if(State == 1) { //StabFlag = 0,start detect the change of rect num
				if(RectNum[0] != RectNum[1]) {  //find the change of rect,and begin to waiting the rect stable
					State = 2;
				} 

			} else
			if(State == 2) {
				WaitFlag++;
				if(WaitFlag == 5) { //在检测到框的数量发生变化之后的5帧图片之后进行判断其是框增加还是减少
					if(RectNum[0] < RectNumTemp) { 
						State = 3;
					} else {
						State = 0;
						WaitFlag = 0;
					}
				}
			} else
			if(State == 3) {
				if(RectNum[0] <= RectNum[1]) {
					WaitFlag++;
				} else {     //the rect num increase, back to sure the num stable
					WaitFlag = 0;
					State = 0;
				}
				if(WaitFlag == 8) { //在检测到框的数量减少后的5帧图片后框的数量不再发生变化则画框
					SendFlag = 1;
				}
			}

			printf("State:%d!!! WaitFlag:%d \n",State,WaitFlag);

			if(SendFlag == 1) { 
				UartSendRead_pipeflaw(uartFd);
				State = 0;
				SendFlag = 0;
				WaitFlag = 0;
				StabFlag = 0;
            	printf("***************find pipeflaw!!! \n");
			}

			printf("The RectNum:%d !!!\n",RectNum[0]);
			
			release_result(output_result, final_result); // Free memory, end one frame
			S_frame_flag = 1;
			return s32Ret;
		}
	
	
	
		/******* deal with coordinates *********/
	 /*
		
#ifdef NNIE_DEBUG
		printf("Frank : pstSwParam->stRect.u32TotalNum = %d \r\n ",pstSwParam->stRect.u32TotalNum);   //Frank : pstSwParam->stRect.u32TotalNum = 0
#endif
	
		if(pstSwParam->stRect.u32TotalNum == 0)
		{
			return HI_SUCCESS;
		}
		else
		{
			BoxInfo u_temp;
			for (int i = 0; i < pstSwParam->stRect.u32ClsNum; i++)
			{
				for (int j = 0; j < pstSwParam->stRect.au32RoiNum[i]; j++)
				{
					u_temp.score = 0.6f;
			
					u_temp.box.x = pstSwParam->stRect.astRect[i][j].astPoint[0].s32X; 
					u_temp.box.y = pstSwParam->stRect.astRect[i][j].astPoint[0].s32Y; 
					u_temp.box.width = (pstSwParam->stRect.astRect[i][j].astPoint[1].s32X - pstSwParam->stRect.astRect[i][j].astPoint[0].s32X); 
					u_temp.box.height = (pstSwParam->stRect.astRect[i][j].astPoint[2].s32Y - pstSwParam->stRect.astRect[i][j].astPoint[0].s32Y); 
			
			/*
					u_temp.box.x = 0 ;
					u_temp.box.y = 0 ;
					u_temp.box.width = 0;
					u_temp.box.height = 0;
			*/
		   /*		  u_temp.label = i;  //14
	
	
	#ifdef NNIE_DEBUG
			printf("@@@@@@@@@@@xxxxxxxxxxxxxxxxxxxx################# \r\n");
	
			printf("u_temp.box.x = %4d, u_temp.box.y = %4d \r\n",pstSwParam->stRect.astRect[i][j].astPoint[0].s32X,pstSwParam->stRect.astRect[i][j].astPoint[0].s32Y);
			//printf("astPoint[2].s32X = %4d, astPoint[3].s32X = %4d \r\n",pstSwParam->stRect.astRect[i][j].astPoint[2].s32X,pstSwParam->stRect.astRect[i][j].astPoint[3].s32X);
			printf("\r\n");
	
				printf("u_temp.box.width = %4d, u_temp.box.height = %4d \r\n",(pstSwParam->stRect.astRect[i][j].astPoint[1].s32X - pstSwParam->stRect.astRect[i][j].astPoint[0].s32X),(pstSwParam->stRect.astRect[i][j].astPoint[2].s32Y - pstSwParam->stRect.astRect[i][j].astPoint[0].s32Y));
			
	#endif
	
					bboxs.push_back(u_temp);
			
				
				}
			}
		}
	
		/*
		if (HI_MPI_VPSS_GetChnFrame(0, 0, &stFrame_vo, 20000) != HI_SUCCESS)  //HI_MPI_VPSS_GetChnFrame(0, 0, &stFrame_vo, s32MilliSec)
		{
			printf("Get frame_vo fail \n");
			usleep(1000);
			return -1;
		}
	
		
			//Draw rect
			//s32Ret = SAMPLE_COMM_SVP_NNIE_FillRect(&stFrame_vo, &(pstSwParam->stRect), 0x0000FF00);
		s32Ret = MY_COMM_SVP_NNIE_FillRect(&stFrame_vo, &(pstSwParam->stRect), 0x0000FF00);
			SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS!=s32Ret, BASE_RELEASE,
				"SAMPLE_COMM_SVP_NNIE_FillRect failed, Error(%#x)!\n", s32Ret);
		
			s32Ret = HI_MPI_VO_SendFrame(0, 0, &stFrame_vo, s32MilliSec);
		//HI_MPI_VPSS_SendFrame(0, 0, &stFrame_vo, s32MilliSec);
			SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS!=s32Ret, BASE_RELEASE,
				"xxx HI_MPI_VO_SendFrame failed, Error(%#x)!\n", s32Ret);
	
		s32Ret = HI_MPI_VPSS_ReleaseChnFrame(0, 0, &stFrame_vo);
			if (HI_SUCCESS != s32Ret)
			{
			printf("Release frame_vo error ,now exit !!!\n");
			//VPSS_Restore(0, 0);
			return -1;
			}
		*/	
	
		// return s32Ret;
		BASE_RELEASE:
			s32Ret = HI_MPI_VPSS_ReleaseChnFrame(s32VpssGrp, s32VpssChn, pstFrame);
			if (HI_SUCCESS != s32Ret)
			{
				SAMPLE_PRT("Error(%#x),HI_MPI_VPSS_ReleaseChnFrame failed,Grp(%d) chn(%d)!\n",
					s32Ret,s32VpssGrp,s32VpssChn);
			}
		/*
		s32Ret = HI_MPI_VPSS_ReleaseChnFrame(0, 0, &stFrame_vo);
			if (HI_SUCCESS != s32Ret)
			{
				SAMPLE_PRT("Error(%#x),HI_MPI_VPSS_ReleaseChnFrame failed,Grp(%d) chn(%d)!\n",
					s32Ret, 0, 0);
			}
		*/
	
		return HI_FAILURE;
	
	
	
	}
	

	int yolov5_Detect() //std::vector<BoxInfo> &bboxs
	{	
		if (HI_MPI_VPSS_GetChnFrame(VpssGrp, VpssChn, &stFrame, 20000) != HI_SUCCESS)  //HI_MPI_VPSS_GetChnFrame(VpssGrp, VpssChn, &stFrame, s32MilliSec)
		{
			printf("Get frame fail \n");
			usleep(1000);
			return -1;
		}
	
	
		/* Test for NNIE input image pixel*/
#ifdef NNIE_DEBUG
		VPSS_CHN_ATTR_S NNIE_Chn_attr;
		HI_MPI_VPSS_GetChnAttr(0, 0, &NNIE_Chn_attr);
		printf("Grp0 = %d, Chn0 = %d, NNIE_Chn_attr.u32Width = %d , NNIE_Chn_attr.u32Height = %d\r\n",0, 0, NNIE_Chn_attr.u32Width, NNIE_Chn_attr.u32Height);
	
		
		HI_MPI_VPSS_GetChnAttr(VpssGrp, VpssChn, &NNIE_Chn_attr);
		printf("VpssGrp = %d, VpssChn = %d, NNIE_Chn_attr.u32Width = %d , NNIE_Chn_attr.u32Height = %d\r\n",VpssGrp, VpssChn, NNIE_Chn_attr.u32Width, NNIE_Chn_attr.u32Height);
	
		//HI_MPI_VO_GetPubAttr(0, VO_PUB_ATTR_S *pstPubAttr);
	
#endif
		
	
	
		/*
	#ifdef NNIE_DEBUG1
		
		printf("############################### \r\n ");
		printf("Frank : stFrame.stVFrame.enPixelFormat = %d \r\n ", stFrame.stVFrame.enPixelFormat);	//stVFrame.enPixelFormat = 26
		printf("############################### \r\n ");
			   
#endif
		*/	
	
		
		bool bSendToVgs = ((COMPRESS_MODE_NONE != stFrame.stVFrame.enCompressMode) || (VIDEO_FORMAT_LINEAR != stFrame.stVFrame.enVideoFormat));
	
		if (bSendToVgs){
			return -1;
		}
	
		/*
		if (DYNAMIC_RANGE_SDR8 == stFrame.stVFrame.enDynamicRange)
		{
			int ret = YUV2Mat(&stFrame.stVFrame, frame);	
			if(ret < 0) return -1;
		}
		*/
		
		if (yolov5_Proc(VpssGrp, VpssChn, &stFrame) != HI_SUCCESS)	//My_Rfcn_Detect(VpssGrp, VpssChn, &stFrame, bboxs) 
		{
			printf("yolov5_Proc fail \n");
			usleep(1000);
			return -1;
		}
			
		//printf("HSJ Debug befor VENC send frame\n");
		HI_S32 s32Ret = HI_MPI_VENC_SendFrame(VencChn[1], &stFrame, 20000) ;
		if (HI_SUCCESS != s32Ret)
		{
			printf("HI_MPI_VENC_SendFrame channel 0 failed, Error(%#x)!\n", s32Ret);
			return -1;
		}
		if(frame_id <= 20000) {  //存储前20000帧以内的视频，大概10分钟,超出10分钟停止存储，但注意防止时间太久，导致
			//HI_S32 s32Ret = HI_MPI_VENC_SendFrame(VencChn[0], &stFrame, 20000) ;
			if (HI_SUCCESS != s32Ret)
			{
				printf("HI_MPI_VENC_SendFrame channel 0 failed, Error(%#x)!\n", s32Ret);
				return -1;
			}
		}
		/*
		s32Ret = h264ToRTMP_timing();
		if (s32Ret == -1)
		{
			printf("h264ToRTMP_timing failed, Error!\n");
			return -1;
		}
		*/
		//printf("HSJ Debug befor vo send frame\n");
		s32Ret = HI_MPI_VO_SendFrame(0, 0, &stFrame, 20000);//(voLayer, voChn, &stBaseFrmInfo, s32MilliSec)
		if (HI_SUCCESS != s32Ret)
		{
			printf("HI_MPI_VO_SendFrame failed, Error(%#x)!\n", s32Ret);
			//VPSS_Restore(VpssGrp, VpssChn);
			return -1;
		}
	
		printf("Send frame %d!!\n", frame_id);
		/* release frame after using */
		s32Ret = HI_MPI_VPSS_ReleaseChnFrame(VpssGrp, VpssChn, &stFrame);
	
		if (HI_SUCCESS != s32Ret)
		{
			printf("Release frame error ,now exit !!!\n");
			//VPSS_Restore(VpssGrp, VpssChn);
			return -1;
		}
		
		if(frame_id == 0){
			start = clock();
		}
		
		if(frame_id == 1000){
			finish = clock();
			printf( "1000 frame process used:%f seconds\n", (double)(finish - start)/CLOCKS_PER_SEC);
		}
		if(frame_id < 20000) {
			frame_id++;
		}
		return 0;
	
	
	}
	
	
	
	
	void yolov5_Parm_init()
	{
		memset(&s_stYOLOv5Model,0,sizeof(s_stYOLOv5Model));
		memset(&s_stYOLOv5NnieParam,0,sizeof(s_stYOLOv5NnieParam));
		//memset(&s_stRfcnSoftwareParam,0,sizeof(s_stRfcnSoftwareParam));
	}
	
	
	
	HI_S32 yolov5_init(HI_CHAR * ModelFile)
	{
		SAMPLE_SVP_NNIE_CFG_S	stNnieCfg = {0};
		HI_S32 s32Ret = HI_SUCCESS;
	
		/*************	NnieCfg **************************/
		stNnieCfg.pszPic= NULL;
		stNnieCfg.u32MaxInputNum = 1; //max input image num in each batch
		stNnieCfg.u32MaxRoiNum = 0;  //300??
		stNnieCfg.aenNnieCoreId[0] = SVP_NNIE_ID_0; //set NNIE core for 0-th Seg
		//stNnieCfg.aenNnieCoreId[1] = SVP_NNIE_ID_0; //set NNIE core for 1-th Seg
		//stNnieCfg.aenNnieCoreId[2] = SVP_NNIE_ID_0; //set NNIE core for 2-th Seg
	
		yolo_result *output_result = NULL;
	
		/*Sys init*/
		//SAMPLE_COMM_SVP_CheckSysInit();  // // 这部分在sample_vdec_hsj中已经定义了,vb的初始化
	
		/********  My_RfcnParm_init ************/
		yolov5_Parm_init();
		
		/********  SVP_NNIE_LoadModel ************/
		SAMPLE_SVP_TRACE_INFO("YOLO Load model!\n");//???
		s32Ret = SAMPLE_COMM_SVP_NNIE_LoadModel(ModelFile,&s_stYOLOv5Model);//s_stYOLOModel have the model
		SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret,END_YOLOv5_0,SAMPLE_SVP_ERR_LEVEL_ERROR,
			"Error,SAMPLE_COMM_SVP_NNIE_LoadModel failed!\n");
		
		/********  YOLOv5_ParamInit   *********/
		SAMPLE_SVP_TRACE_INFO("Cnn parameter initialization!\n");
		s_stYOLOv5NnieParam.pstModel = &s_stYOLOv5Model.stModel; // Network model structure: stModel
		
		//s_stRfcnNnieParam.pstModel = &s_stRfcnModel.stModel;
		//s_stRfcnSoftwareParam.apcRpnDataLayerName[0] = "rpn_cls_score";
		//s_stRfcnSoftwareParam.apcRpnDataLayerName[1] = "rpn_bbox_pred";
		//s32Ret = My_Rfcn_ParamInit(&stNnieCfg,&s_stRfcnNnieParam,&s_stRfcnSoftwareParam);
		s32Ret = SAMPLE_SVP_NNIE_YOLOv5_ParamInit(&stNnieCfg, &s_stYOLOv5NnieParam); // Initialize NNIE hardware layer parameters
		SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret,END_YOLOv5_1,SAMPLE_SVP_ERR_LEVEL_ERROR,
			"Error,YOLOv5_ParamInit failed!\n");
		
		
		printf("NNIE init oK\r\n"); 
		return s32Ret;
		
	END_YOLOv5_1:
		SAMPLE_SVP_NNIE_YOLOv5_Deinit(&s_stYOLOv5NnieParam, NULL);
	END_YOLOv5_0:
		return HI_FAILURE;
	}
	
	
	
	int vcap_init(int VpssGrp, int VpssChn)
	{
		HI_CHAR szPixFrm[10];
		HI_U32 u32Depth = 2;
		HI_S32 s32Times = 10;
		VIDEO_FRAME_INFO_S stFrmInfo;
		VGS_TASK_ATTR_S stTask;
		HI_U32 u32LumaSize = 0;
		HI_U32 u32PicLStride = 0;
		HI_U32 u32PicCStride = 0;
		HI_U32 u32Width = 0;
		HI_U32 u32Height = 0;
		HI_S32 i = 0;
		HI_S32 s32Ret;
		VPSS_CHN_ATTR_S stChnAttr;
		VPSS_EXT_CHN_ATTR_S stExtChnAttr;
		HI_U32 u32BitWidth;
		VB_POOL_CONFIG_S stVbPoolCfg;
	
	  // signal(SIGINT, VPSS_Chn_Dump_HandleSig);
	  // signal(SIGTERM, VPSS_Chn_Dump_HandleSig);
	
		if (VpssChn > VPSS_CHN3)
		{
			s32Ret = HI_MPI_VPSS_GetExtChnAttr(VpssGrp, VpssChn, &stExtChnAttr);
			u32OrigDepth = stExtChnAttr.u32Depth;
		}
		else
		{
			s32Ret = HI_MPI_VPSS_GetChnAttr(VpssGrp, VpssChn, &stChnAttr);
			u32OrigDepth = stChnAttr.u32Depth;
		}
	
		if (s32Ret != HI_SUCCESS)
		{
			printf("get chn:%d attr error!!!\n", VpssChn);
			return -1;
		}
	
		if (VpssChn > VPSS_CHN3)
		{
			stExtChnAttr.u32Depth = u32Depth;
			s32Ret = HI_MPI_VPSS_SetExtChnAttr(VpssGrp, VpssChn, &stExtChnAttr);
		}
		else
		{
			stChnAttr.u32Depth = u32Depth;
			s32Ret = HI_MPI_VPSS_SetChnAttr(VpssGrp, VpssChn, &stChnAttr) ;
		}
		if (s32Ret != HI_SUCCESS)
		{
			printf("set depth error!!!\n");
			//VPSS_Restore(VpssGrp, VpssChn);
			return -1;
		}
		u32VpssDepthFlag = 1;
	
		memset(&stFrame, 0, sizeof(stFrame));
		stFrame.u32PoolId = VB_INVALID_POOLID;
		while (HI_MPI_VPSS_GetChnFrame(VpssGrp, VpssChn, &stFrame, s32MilliSec) != HI_SUCCESS)
		{
			s32Times--;
			if (0 >= s32Times)
			{
				printf("get frame error for 10 times,now exit !!!\n");
				//VPSS_Restore(VpssGrp, VpssChn);
				return -1;
			}
			usleep(40*1000);
		}
	
		if (VIDEO_FORMAT_LINEAR != stFrame.stVFrame.enVideoFormat)
		{
			printf("only support linear frame dump!\n");
			HI_MPI_VPSS_ReleaseChnFrame(VpssGrp, VpssChn, &stFrame);
			stFrame.u32PoolId = VB_INVALID_POOLID;
			return -1;
		}
	
		switch (stFrame.stVFrame.enPixelFormat)
		{
			case PIXEL_FORMAT_YVU_SEMIPLANAR_420:
			case PIXEL_FORMAT_YUV_SEMIPLANAR_420:
				snprintf(szPixFrm, 10, "P420");
				break;
	
			case PIXEL_FORMAT_YVU_SEMIPLANAR_422:
			case PIXEL_FORMAT_YUV_SEMIPLANAR_422:
				snprintf(szPixFrm, 10, "P422");
				break;
	
			default:
				snprintf(szPixFrm, 10, "P400");
				break;
		}
		
	
		s32Ret = HI_MPI_VPSS_ReleaseChnFrame(VpssGrp, VpssChn, &stFrame);
	
		if (HI_SUCCESS != s32Ret)
		{
			printf("vcap_init ---> Release frame error ,now exit !!!\n");
			return -1;
		}
	
		//this->VpssGrp = VpssGrp;
		//this->VpssChn = VpssChn;
		return 0;
	}
	
	
	
	
	int yolo_init(int VpssGrp, int VpssChn, char *ModelPath)
	{
		int i = 10;
		int ret = 0;

		uartFd = UartOpenInit(); //open the UART
    		if (uartFd < 0) {
       			printf("uart1 open failed\r\n");
    		} else {
        		printf("uart1 open successed\r\n");
    		}
		
		do{
		  ret = vcap_init(VpssGrp, VpssChn);
		  //printf("vcap.init ret:%d\n", ret);
		  if(ret == 0)
		  {
			ret = yolov5_init(ModelPath);
			//My_Rfcn_Init(ModelPath);
			printf("yolov5_init ret:%d\n", ret);
			break;
		  }
		  sleep(1);
		}while(i-- > 0);
		
		return ret;
	}
	int yolo_detect() //yolo_boxs_t *boxs
	{
		static const char* class_names[] = {"pipeflaw"};
		
		/*vcap.get_frame(image);
		if(image.empty())
		{
			std::cout << "vpss capture failed!!!\n";
			return -1;
		}*/
	
	
		//std::vector<BoxInfo> bboxs;
		  
		//int ret = yolov5.detect(image, bboxs);
	
		//int ret = My_Frame_Detect(bboxs);
		int ret = yolov5_Detect();

		return 0;
	}
	int yolo_deinit()
	{
		/*
	  yolov5.destroy();
	  vcap.destroy();
	  */
	  SAMPLE_SVP_NNIE_YOLOv5_Deinit(&s_stYOLOv5NnieParam,&s_stYOLOv5Model);
	  return 0;
	}

